import ctypes
import os
from ctypes import *
import struct

# # 编译C代码
# os.system('gcc -shared -o /home/teamhd/sdk/src/libtcpmodbus_master.so -fPIC /home/teamhd/sdk/src/tcpmodbus_master.c -pthread')
# os.system('gcc -o /home/teamhd/sdk/src/tcpmodbus_server /home/teamhd/sdk/src/tcpmodbus_server.c -pthread')

# 加载共享库
# master_lib = CDLL('/home/teamhd/sdk/src/libtcpmodbus_master.so')
master_lib = CDLL('src/libtcpmodbus_master.so')

# 定义函数原型
master_lib.modbus_read_coils.argtypes = [c_uint8, c_uint16, c_uint16, POINTER(c_uint8)]
master_lib.modbus_read_coils.restype = None

master_lib.modbus_read_discrete_inputs.argtypes = [c_uint8, c_uint16, c_uint16, POINTER(c_uint8)]
master_lib.modbus_read_discrete_inputs.restype = None

master_lib.modbus_read_holding_registers.argtypes = [c_uint8, c_uint16, c_uint16, POINTER(c_uint16)]
master_lib.modbus_read_holding_registers.restype = None

master_lib.modbus_read_input_registers.argtypes = [c_uint8, c_uint16, c_uint16, POINTER(c_uint16)]
master_lib.modbus_read_input_registers.restype = None

master_lib.modbus_write_single_coil.argtypes = [c_uint8, c_uint16, c_uint8]
master_lib.modbus_write_single_coil.restype = None

master_lib.modbus_write_single_register.argtypes = [c_uint8, c_uint16, c_uint16]
master_lib.modbus_write_single_register.restype = None

master_lib.modbus_write_multiple_coils.argtypes = [c_uint8, c_uint16, c_uint16, POINTER(c_uint8)]
master_lib.modbus_write_multiple_coils.restype = None

master_lib.modbus_write_multiple_registers.argtypes = [c_uint8, c_uint16, c_uint16, POINTER(c_uint16)]
master_lib.modbus_write_multiple_registers.restype = None

class ModbusMaster():
    def read_coils(self, unit_id, start_addr, quantity):
        """读取线圈状态 (功能码0x01)"""
        values = (c_uint8 * ((quantity + 7) // 8))()
        master_lib.modbus_read_coils(unit_id, start_addr, quantity, values)
        return [values[i] for i in range((quantity + 7) // 8)]
    
    def read_discrete_inputs(self, unit_id, start_addr, quantity):
        """读取离散输入 (功能码0x02)"""
        values = (c_uint8 * ((quantity + 7) // 8))()
        master_lib.modbus_read_discrete_inputs(unit_id, start_addr, quantity, values)
        return [values[i] for i in range((quantity + 7) // 8)]
    
    def read_holding_registers(self, unit_id, start_addr, quantity):
        try:
            """读取保持寄存器 (功能码0x03)"""
            values = (c_uint16 * quantity)()
            master_lib.modbus_read_holding_registers(unit_id, start_addr, quantity, values)
            return [values[i] for i in range(quantity)]
        except Exception as e:
            print(e)
    
    def read_input_registers(self, unit_id, start_addr, quantity):
        """读取输入寄存器 (功能码0x04)"""
        values = (c_uint16 * quantity)()
        master_lib.modbus_read_input_registers(unit_id, start_addr, quantity, values)
        return [values[i] for i in range(quantity)]
    
    def write_single_coil(self, unit_id, address, value):
        """写入单个线圈 (功能码0x05)"""
        master_lib.modbus_write_single_coil(unit_id, address, 1 if value else 0)
    
    def write_single_register(self, unit_id, address, value):
        """写入单个寄存器 (功能码0x06)"""
        master_lib.modbus_write_single_register(unit_id, address, value)
    
    def write_multiple_coils(self, unit_id, start_addr, coil_values):
        """写入多个线圈 (功能码0x0F)"""
        quantity = len(coil_values)
        byte_count = (quantity + 7) // 8
        packed_values = [0] * byte_count
        
        for i in range(quantity):
            if coil_values[i]:
                packed_values[i // 8] |= (1 << (i % 8))
        
        c_values = (c_uint8 * byte_count)(*packed_values)
        master_lib.modbus_write_multiple_coils(unit_id, start_addr, quantity, c_values)
    
    def write_multiple_registers(self, unit_id, start_addr, values):
        """写入多个寄存器 (功能码0x10)"""
        quantity = len(values)
        c_values = (c_uint16 * quantity)(*values)
        master_lib.modbus_write_multiple_registers(unit_id, start_addr, quantity, c_values)


    def get_DI(self, value):
        try:
            value_tmp = 32 + value
            value_dst = self.read_holding_registers(1, value_tmp, 1)
            return value_dst[0]
        except Exception as e:
            print(e)

    def get_DO(self, value):
        try:
            value_tmp = 28 + value
            value_dst = self.read_holding_registers(1, value_tmp, 1)
            return value_dst[0]
        
        except Exception as e:
            print(e)
        
    def get_Motor(self, value):
        try:
            value = 44 + value
            value = self.read_holding_registers(1, value, 1)
            return value[0]
        
        except Exception as e:
            print(e)

    def read_status(self, offset):
        value = self.read_holding_registers(1, offset, 1)
        return value[0] if value else None

    def read_robot_type(self, offset):
        value = self.read_holding_registers(1, offset, 1)
        return value[0] if value else None

    def read_pose_floats(self, starting_address):
        motion = self.read_holding_registers(1, starting_address, 26)
        if not motion:
            return []

        floats = []
        for i in range(0, 26, 2):
            if i + 1 < len(motion):
                floats.append(self.int2float(motion[i], motion[i+1]))
            else:
                print("Error: Not enough integers to convert to floats.")
                break
        return floats

    def get_status(self,robotNum):
        try:
            if robotNum == 1:
                robot1_status = self.read_status(0)
                return robot1_status
            elif robotNum == 2:
                robot2_status = self.read_status(60)
                return robot2_status
            elif robotNum == 3:
                robot3_status = self.read_status(100)
                return robot3_status
        
        except Exception as e:
            print(e)
    
    def get_robotType(self,robotNum):
        try:
            if robotNum == 1:
                robot1_type = self.read_robot_type(27)
                return robot1_type
            elif robotNum == 2:
                robot2_type = self.read_robot_type(87)
                return robot2_type
            elif robotNum == 3:
                robot3_type = self.read_robot_type(127)
                return robot3_type
            
        except Exception as e:
            print(e)
    
    def get_pose_floats(self, robotNum):
        try:
            if robotNum == 1:
                robot1_pose = self.read_pose_floats(1)
                return robot1_pose
            elif robotNum == 2:
                robot2_pose = self.read_pose_floats(61)
                return robot2_pose
            elif robotNum == 3:
                robot3_pose = self.read_pose_floats(101)
                return robot3_pose
        
        except Exception as e:
            print(e)

    def get_hold_register(self,address, num):
        value = self.read_holding_registers(1, address, num)
        return value if value else None

    def get_coils_register(self, address, num):
        value = self.read_coils(1, address, num)
        return value if value else None
    
    ### 读取寄存器值，转换为float显示应用
    def int2float(self,a,b):
        value=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            value=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
            if value is None:
                return value
            if isinstance(value, float):
                # 精确到小数点后两位数
                return round(value , 2)
            else:
                return value
        except BaseException as e:
            print(e)
        return value